package anyVehicle;

import java.util.HashMap;
import java.util.HashSet;
import java.util.Iterator;
import java.util.Map;
import java.util.Set;

import javax.vecmath.Vector3f;

import net.minecraft.src.Entity;
import net.minecraft.src.ModLoader;
import net.minecraft.src.World;

import com.bulletphysics.BulletStats;
import com.bulletphysics.collision.broadphase.AxisSweep3_32;
import com.bulletphysics.collision.broadphase.BroadphaseInterface;
import com.bulletphysics.collision.broadphase.DbvtBroadphase;
import com.bulletphysics.collision.dispatch.CollisionDispatcher;
import com.bulletphysics.collision.dispatch.CollisionObject;
import com.bulletphysics.collision.dispatch.DefaultCollisionConfiguration;
import com.bulletphysics.collision.dispatch.GhostPairCallback;
import com.bulletphysics.collision.narrowphase.ManifoldPoint;
import com.bulletphysics.collision.narrowphase.PersistentManifold;
import com.bulletphysics.collision.shapes.BoxShape;
import com.bulletphysics.collision.shapes.CollisionShape;
import com.bulletphysics.dynamics.DiscreteDynamicsWorld;
import com.bulletphysics.dynamics.DynamicsWorld;
import com.bulletphysics.dynamics.InternalTickCallback;
import com.bulletphysics.dynamics.RigidBody;
import com.bulletphysics.dynamics.character.KinematicCharacterController;
import com.bulletphysics.dynamics.constraintsolver.ConstraintPersistentData;
import com.bulletphysics.dynamics.constraintsolver.ConstraintSolver;
import com.bulletphysics.dynamics.constraintsolver.ContactConstraint;
import com.bulletphysics.dynamics.constraintsolver.ContactSolverInfo;
import com.bulletphysics.dynamics.constraintsolver.SequentialImpulseConstraintSolver;
import com.bulletphysics.linearmath.Clock;
import com.bulletphysics.linearmath.DefaultMotionState;
import com.bulletphysics.linearmath.Transform;

public class JBulletSimulation {
	private JBulletBodyManager bodyManager;
	private BroadphaseInterface broadphase;
	private CollisionDispatcher dispatcher;
	private ConstraintSolver solver;
	private DefaultCollisionConfiguration collisionConfiguration;
	protected Clock clock = new Clock();
	protected DynamicsWorld dynamicsWorld;
	private static Vector3f tmpVector = new Vector3f();
	private static Transform tmpTrans1 = new Transform();
	private static Transform tmpTrans2 = new Transform();
	public static float FIXED_SIM_STEP_SCALE = 0.1f;

	public JBulletBodyManager getBodyManager() {
		return bodyManager;
	}

	private final class VehiclePhysicsInternalTickCallback extends InternalTickCallback {
		private Set<RigidBody> collidedBodies = new HashSet<RigidBody>();

		@Override
		public void internalTick(DynamicsWorld world, float timeStep) {
			collidedBodies.clear();
			for (int i = 0; i < dispatcher.getNumManifolds(); i++) {
				PersistentManifold manifold = dispatcher.getManifoldByIndexInternal(i);
				CollisionObject obj1 = (CollisionObject) manifold.getBody0();
				CollisionObject obj2 = (CollisionObject) manifold.getBody1();
				if (obj1.getUserPointer() instanceof Vehicle && obj2.getUserPointer() instanceof Vehicle) {
					// It does not process Vehicle-Vehicle collisions
					continue;
				}

				if (obj1.getUserPointer() instanceof Entity && obj2.getUserPointer() instanceof Entity) {
					// removePenetration((RigidBody) obj1, (RigidBody) obj2,
					// manifold, timeStep);
					// processManifold(manifold, obj1, obj2);

				}
			}
			for (RigidBody body : collidedBodies) {
				// body.getLinearVelocity(tmpVector);
				// bodyManager.translateKinematicBody(body, tmpVector.x *
				// timeStep, tmpVector.y * timeStep, tmpVector.z * timeStep);

//				Entity entity = (Entity) body.getUserPointer();
//				body.updateEntityFromKinematicBody();
				// entity.setVelocity(tmpVector.x, tmpVector.y, tmpVector.z);

			}
		}

		private void printContactPoints(PersistentManifold manifold) {
			for (int j = 0; j < manifold.getNumContacts(); j++) {
				ManifoldPoint pt = manifold.getContactPoint(j);
				Vector3f ptA = new Vector3f();
				Vector3f ptB = new Vector3f();
				pt.getPositionWorldOnA(ptA);
				pt.getPositionWorldOnB(ptB);
				CollisionObject obj1 = (CollisionObject) manifold.getBody0();
				CollisionObject obj2 = (CollisionObject) manifold.getBody1();

				System.out.println(j + ": " + obj1.getUserPointer() + " -> " + obj2.getUserPointer() + "   " + pt.getDistance() + " " + pt.normalWorldOnB + " "
						+ " --- " + ptA + "  " + ptB);
				System.out.flush();
			}

		}

		public void processManifold(PersistentManifold manifold, CollisionObject obj1, CollisionObject obj2) {
			for (int j = 0; j < manifold.getNumContacts(); j++) {
				ManifoldPoint pt = manifold.getContactPoint(j);
				Vector3f ptA = new Vector3f();
				Vector3f ptB = new Vector3f();
				pt.getPositionWorldOnA(ptA);
				pt.getPositionWorldOnB(ptB);

				System.out.println(obj1.getUserPointer() + " -> " + obj2.getUserPointer() + "   " + pt.getDistance() + " " + pt.normalWorldOnB + " " + " --- "
						+ ptA + "  " + ptB);
				System.out.flush();

				RigidBody body1 = RigidBody.upcast(obj1);
				RigidBody body2 = RigidBody.upcast(obj2);

				collidedBodies.add(body1);
				collidedBodies.add(body2);

				if (body1.getUserPointer() instanceof Vehicle) {
					// float dx = (ptA.x - ptB.x);
					// float dy = (ptA.y - ptB.y);
					// float dz = (ptA.z - ptB.z);
					// Vector3f normal = new Vector3f(pt.normalWorldOnB);
					// normal.scale(pt.getDistance());
					// bodyManager.translateKinematicBody(body2, normal.x,
					// normal.y, normal.z);

					// collisionResponseUsingSolver(body1, body2, pt, timeStep);
				} else if (body2.getUserPointer() instanceof Vehicle) {
					// bodyManager.translateKinematicBody(body1, -dx, -dy, -dz);

				}

			}
		}
	}

	public JBulletSimulation() {
		initPhysics();

	}

	public void initPhysics() {
		// collision configuration contains default setup for memory, collision
		// setup
		collisionConfiguration = new DefaultCollisionConfiguration();

		// use the default collision dispatcher. For parallel processing you can
		// use a diffent dispatcher (see Extras/BulletMultiThreaded)
		dispatcher = new CollisionDispatcher(collisionConfiguration);

		// broadphase = new DbvtBroadphase();
		Vector3f worldMin = new Vector3f(-32000f, -32000f, -32000f);
		Vector3f worldMax = new Vector3f(32000f, 32000f, 32000f);
		AxisSweep3_32 sweepBP = new AxisSweep3_32(worldMin, worldMax);
		broadphase = sweepBP;
		// the default constraint solver. For parallel processing you can use a
		// different solver (see Extras/BulletMultiThreaded)
		SequentialImpulseConstraintSolver sol = new SequentialImpulseConstraintSolver();
		solver = sol;

		dynamicsWorld = new DiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
		dynamicsWorld.getBroadphase().getOverlappingPairCache().setInternalGhostPairCallback(new GhostPairCallback());
		bodyManager = new JBulletBodyManager(dynamicsWorld);

		dynamicsWorld.setGravity(new Vector3f(0f, -10f, 0f));

		resetScene();

		// dynamicsWorld.setInternalTickCallback(
		// new VehiclePhysicsInternalTickCallback(), null);
	}

	public void resetScene() {
		BulletStats.gNumDeepPenetrationChecks = 0;
		BulletStats.gNumGjkChecks = 0;

		int numObjects = 0;
		if (dynamicsWorld != null) {
			dynamicsWorld.stepSimulation(1f / 60f, 0);
			numObjects = dynamicsWorld.getNumCollisionObjects();
		}

		for (int i = 0; i < numObjects; i++) {
			CollisionObject colObj = dynamicsWorld.getCollisionObjectArray().getQuick(i);
			RigidBody body = RigidBody.upcast(colObj);
			if (body != null) {
				if (body.getMotionState() != null) {
					DefaultMotionState myMotionState = (DefaultMotionState) body.getMotionState();
					myMotionState.graphicsWorldTrans.set(myMotionState.startWorldTrans);
					colObj.setWorldTransform(myMotionState.graphicsWorldTrans);
					colObj.setInterpolationWorldTransform(myMotionState.startWorldTrans);
					colObj.activate();
				}
				// removed cached contact points
				dynamicsWorld.getBroadphase().getOverlappingPairCache().cleanProxyFromPairs(colObj.getBroadphaseHandle(), dynamicsWorld.getDispatcher());

				body = RigidBody.upcast(colObj);
				if (body != null && !body.isStaticObject()) {
					RigidBody.upcast(colObj).setLinearVelocity(new Vector3f(0f, 0f, 0f));
					RigidBody.upcast(colObj).setAngularVelocity(new Vector3f(0f, 0f, 0f));
				}
			}

		}
	}

	public float getDeltaTimeMicroseconds() {
		float dt = clock.getTimeMicroseconds();
		clock.reset();
		return dt;
	}

	public void tick() {
		if (dynamicsWorld != null) {
			
			float ms = 0.05f;
//			dynamicsWorld.stepSimulation(ms);
			dynamicsWorld.stepSimulation(ms, 10, ms * FIXED_SIM_STEP_SCALE);
//			RigidBody body = kinematicControllers.get(ModLoader.getMinecraftInstance().thePlayer);
//			if (body != null)
//				System.out.println(body.getMotionState().getWorldTransform(new Transform()).origin);
		}

	}



	private Transform getTransformFromEntity(Entity entity) {
		Transform t = new Transform();
		t.origin.set((float) entity.posX, (float) entity.posY, (float) entity.posZ);
		return t;
	}

	public void beforeUpdateEntities(World world) {

	}

	public void afterUpdateEntities(World world) {

		for (Iterator<EntityKinematicController> it = bodyManager.getKinematicControllers().values().iterator(); it.hasNext();) {
			EntityKinematicController c = it.next();
			Entity entity = (Entity) c.getEntity();
			if (!entity.isDead) {
				c.updateKinematicControllerFromEntity();
			} else {
				it.remove();
				bodyManager.removeKinematicController(c);
			}
		}

		tick();

		for (EntityKinematicController c : bodyManager.getKinematicControllers().values()) {
			if (c.hasCollided()) {
				System.out.println(c.getEntity());
				c.updateEntityFromKinematicController();
				c.collisionReset();
			}
		}
		
		for (EntityRigidBody body : bodyManager.getVehicleBodies()) {
			body.updateEntityFromRigidBody();
		}
	}




//
//	public void collisionResponseUsingSolver(RigidBody body1, RigidBody body2, ManifoldPoint pt, float timeStep) {
//		body2.getLinearVelocity(tmpVector);
//		System.out.println("Before Vel: " + tmpVector);
//		bodyManager.kinematicToRigid(body2, 1f);
//
//		ContactSolverInfo info = new ContactSolverInfo();
//		info.timeStep = timeStep;
//		ConstraintPersistentData cpd = new ConstraintPersistentData();
//
//		pt.userPersistentData = cpd;
//		cpd.persistentLifeTime = pt.getLifeTime();
//
//		ContactConstraint.resolveSingleCollision(body1, body2, pt, info);
//		bodyManager.rigidToKinematic(body2);
//		body2.getLinearVelocity(tmpVector);
//		System.out.println("After Vel: " + tmpVector);
//	}

	public void addPhysicsToEntity(Entity entity) {
		bodyManager.getKinematicControllerForEntity(entity);
		
	}

	

}
